package example;


/*
 * Demo.java -
 * Copyright (c) 2005 Carmine Lia
 * e-mail:      carmine.lia@libero.it
 * web-address: http://www.intermedia.sa.it/lia

 *  This program is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU General Public License
 *  as published by the Free Software Foundation; either version 2
 *  of the License, or any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
 *
 */
import java.io.IOException;
// import java.net.URI;
import java.net.URISyntaxException;

import javax.robotics.vecmath.RMatrix4d;
import javax.robotics.engine.robots.DHRobot;
import javax.robotics.engine.robots.Robot;
import javax.robotics.swing.DisplayPanel;
import javax.robotics.vecmath.RVector;

import org.apache.xmlbeans.XmlException;

/**
 * Robot class functions demonstration.
 * 
 * @author Carmine Lia
 * 
 */
public class Demo
{

    public static void main(String[] args) throws XmlException, IOException,
            URISyntaxException
    {
        new Demo();
    }

    private Demo() throws XmlException, IOException, URISyntaxException
    {
        final String newLine = System.getProperty("line.separator");

        final ClassLoader cl = this.getClass().getClassLoader();

        final DisplayPanel out = new DisplayPanel("Demo Output");
        out.setDefaultCloseOperation(DisplayPanel.EXIT_ON_CLOSE);
        
        final double[] qr =
            { Math.PI / 4.0, Math.PI / 4.0, Math.PI / 4.0, Math.PI / 4.0,
                    Math.PI / 4.0, Math.PI / 4.0 };
        final double[] qs =
            { Math.PI / 16.0, 1.0, 1.0, 1.0, 1.0, 1.0 };

/*        final double[] PUMA560_data =
            { 0.0, 0.0, 0.0, 0.0, Math.PI / 2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                    0.0, 0.0, 0.0, 0.0, 0.35, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                    0.0, 0.0, 0.0, 0.0, 0.4318, 0.0, 0.0, 0.0, 0.0, 17.4,
                    -0.3638, 0.006, 0.2275, 0.13, 0.0, 0.0, 0.524, 0.0, 0.539,
                    0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.15005, 0.0203,
                    -Math.PI / 2.0, 0.0, 0.0, 0.0, 4.8, -0.0203, -0.0141, 0.07,
                    0.066, 0.0, 0.0, 0.086, 0.0, 0.0125, 0.0, 0.0, 0.0, 0.0,
                    0.0, 0.0, 0.0, 0.4318, 0.0, Math.PI / 2.0, 0.0, 0.0, 0.0,
                    0.82, 0.0, 0.019, 0.0, 0.0018, 0.0, 0.0, 0.0013, 0.0,
                    0.0018, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                    -Math.PI / 2.0, 0.0, 0.0, 0.0, 0.34, 0.0, 0.0, 0.0, 0.0003,
                    0.0, 0.0, 0.0004, 0.0, 0.0003, 0.0, 0.0, 0.0, 0.0, 0.0,
                    0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.09, 0.0, 0.0,
                    0.032, 0.00015, 0.0, 0.0, 0.00015, 0.0, 0.00004, 0.0, 0.0,
                    0.0, 0.0, 0.0 };
        final double[] PUMA560_data0 =
            { 0.0, 0.0, 0.0, 0.0, Math.PI / 2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                    0.0, 0.0, 0.0, 0.0, 0.35, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
                    0.4318, 0.0, 0.0, 0.0, 0.0, 17.4, -0.3638, 0.006, 0.2275,
                    0.13, 0.0, 0.0, 0.524, 0.0, 0.539, 0.0, 0.0, 0.0, 0.15005,
                    0.0203, -Math.PI / 2.0, 0.0, 0.0, 0.0, 4.8, -0.0203,
                    -0.0141, 0.07, 0.066, 0.0, 0.0, 0.086, 0.0, 0.0125, 0.0,
                    0.0, 0.0, 0.4318, 0.0, Math.PI / 2.0, 0.0, 0.0, 0.0, 0.82,
                    0.0, 0.019, 0.0, 0.0018, 0.0, 0.0, 0.0013, 0.0, 0.0018,
                    0.0, 0.0, 0.0, 0.0, 0.0, -Math.PI / 2.0, 0.0, 0.0, 0.0,
                    0.34, 0.0, 0.0, 0.0, 0.0003, 0.0, 0.0, 0.0004, 0.0, 0.0003,
                    0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.09, 0.0,
                    0.0, 0.032, 0.00015, 0.0, 0.0, 0.00015, 0.0, 0.00004, 0.0 };
*/
/*        final double[] PUMA560_motor =
            { 200e-6, 62.6111, 1.48e-3, (.395 + .435) / 2.0, 200e-6, 107.815,
                    .817e-3, (.126 + .071) / 2.0, 200e-6, 53.7063, 1.38e-3,
                    (.132 + .105) / 2.0, 33e-6, 76.0364, 71.2e-6,
                    (11.2e-3 + 16.9e-3) / 2.0, 33e-6, 71.923, 82.6e-6,
                    (9.26e-3 + 14.5e-3) / 2.0, 33e-6, 76.686, 36.7e-6,
                    (3.96e-3 + 10.5e-3) / 2.0 };
*/
//        final long startTime = System.nanoTime();

        Robot puma = new DHRobot(cl
                .getResourceAsStream("example/conf/puma560.xml"));

        // load robot configuration from the web
        // Robot puma = new DHRobot(new
        // URI("http://www.intermedia.sa.it/lia/jroboop/puma560.xml"));
        // int dof = puma.getDof();
        out.append("====================================================="
                + newLine + "Robot " + puma.getRobotConfiguration().getName()
                + " kinematics" + newLine
                + "====================================================="
                + newLine);
        out.append("Robot joints variables" + newLine
                + puma.getQ().toString("%8.3f" + newLine));
        out.append(newLine + "Robot position" + newLine
                + puma.kine().toString("%8.3f"));

        puma.setQ(qr);
        out.append(newLine + "Robot joints variables" + newLine
                + puma.getQ().toString("%8.3f" + newLine));

        out.append(newLine + "Robot position" + newLine
                + puma.kine().toString("%8.3f"));

        out.append(newLine + "Robot jacobian w/r to base frame" + newLine
                + puma.jacobian().toString("%8.3f"));

        out.append(newLine + "Robot jacobian w/r to tool frame" + newLine
                + puma.jacobian(qr.length).toString("%8.3f"));

        for (int i = 1; i <= qr.length; i++)
        {
            out
                    .append(newLine
                            + "Robot position partial derivative with respect to joint "
                            + i + newLine + puma.dTdqi(i).toString("%8.3f"));

        }
        final RMatrix4d T = new RMatrix4d(puma.kine());
        final RVector vs = new RVector(qs);
        final RVector vr = new RVector(qr);
        out.append(newLine + "Robot inverse kinematics:" + newLine + "q start"
                + newLine + vs.toString("%8.3f") + newLine + "q final"
                + newLine + puma.invKine(T).toString("%8.3f") + newLine
                + "q real" + newLine + vr.toString("%8.3f"));

/*        puma = new DHRobot(6, PUMA560_data);
        out.append(newLine
                + "====================================================="
                + newLine + "Robot PUMA560 dynamics" + newLine
                + "====================================================="
                + newLine);
        out.append("Robot joints variables" + newLine
                + puma.getQ().toString("%8.3f" + newLine));

        out
                .append("Robot Inertia matrix" + newLine
                        + puma.inertia(puma.getQ()));

        out.append("Robot torque" + newLine
                + puma.torque(puma.getQ(), new RVector(6), new RVector(6)));

        out.append("Robot acceleration"
                + newLine
                + puma
                        .acceleration(puma.getQ(), new RVector(6), new RVector(
                                6)));

        puma = new DHRobot(6, PUMA560_data0, PUMA560_motor);
        out.append(newLine
                + "====================================================="
                + newLine + "Robot PUMA560 with motor dynamics" + newLine
                + "====================================================="
                + newLine);
        out.append("Robot joints variables" + newLine
                + puma.getQ().toString("%8.3f" + newLine));

        out
                .append("Robot Inertia matrix" + newLine
                        + puma.inertia(puma.getQ()));

        out.append("Robot torque" + newLine
                + puma.torque(puma.getQ(), new RVector(6), new RVector(6)));

        out.append("Robot acceleration"
                + newLine
                + puma
                        .acceleration(puma.getQ(), new RVector(6), new RVector(
                                6)));

        final long estimatedTime = System.nanoTime() - startTime;
        out.append("Estimated time:" + newLine + estimatedTime / 1e+9 + " [s]");
*/
    }
}
